#!/usr/bin/python2.7
# coding=utf-8

import time
import bluetooth
import struct
import Queue
import threading as th
import wx.lib.newevent
import numpy as np


from vinsch_comm_objects import Parameter, Sample

ResultRxEvent, EVT_RESULT_RX = wx.lib.newevent.NewEvent()
ResultConnectionEvent, EVT_CONNECTION = wx.lib.newevent.NewEvent()

class CMD():
  """ Collection of constants for commands """
  _SYNC = -1
  NOCMD = 0
  CONF  = 1
  SET   = 2
  UP    = 3
  DOWN  = 4
  SETP  = 5
  GET   = 6


class MODE():
  """ Collection of constants for mode """
  NOMODE    = 0
  STARTUP   = 1
  CONFIG_IS = 2
  CONFIG_OS = 3
  IDLE	    = 4
  TOWING    = 5


class Comm(th.Thread):
  SIMULATE = False # Set to true to simulate a bluetooth connection
  DISCONNECTED = 0  
  CONNECTED = 1
  ERR_CONNECTION = 2

  
  bt_addr = "00:06:66:43:11:8D"
  port    = 2
  timeout = 2.0     # Timeout when waiting for data
  conn_timeout = 10 # Timout upon initial connnection
  sock    = bluetooth.BluetoothSocket( bluetooth.RFCOMM )

  def __init__(self, cmd_que, notify_window):
    super(Comm, self).__init__()
    self.cmd_q = cmd_que
    self._nw = notify_window
    self.parameters = {} # Set of parameters
    self._is_connected = False
    self._up_time = 0
    # Redefine the socket if we are simulating
    if Comm.SIMULATE:
      print "*** Simulating bluetooth ***"
      Comm.sock = SimulatedSocket()
    
  def run(self):
    cmd = None
    # Try to connect
    self._up_time, self._is_connected = self.Start()
    
    while True:
      try:
        cmd = self.cmd_q.get(True, 0.05)
      except Queue.Empty:
	      pass

      if cmd==CMD.NOCMD: 
        self.Stop()
        return None # Quit thread

      if not self._is_connected:
        if time.time()-self._up_time>3:
          # Try to reconnect every 3 seconds
          self._up_time, self._is_connected = self.Start()
      elif cmd==CMD._SYNC: 
          self.Sync()
          cmd = None
      elif cmd!=None:
        result = self._SendCmd(cmd)
        wx.PostEvent(self._nw, ResultRxEvent(data=result))
        if cmd!=CMD.GET or result==None:
          cmd = None
        

  def __del__(self):
    """ Closes socket before destroying object. """
    Comm.sock.close()
    Comm.sock = None

  def Stop(self):
    """ Stops communication to the winsch but keeps the socket. """
    Comm.sock.shutdown(2) # Shutdown socket reading and writing.
    wx.PostEvent(self._nw, ResultConnectionEvent(data=Comm.DISCONNECTED))
  
  def Start(self):
    """ Connect to winch. Returns true on success and false othervice. """
    try:
      Comm.sock.settimeout(Comm.conn_timeout)
      Comm.sock.connect((Comm.bt_addr, Comm.port))
    except bluetooth.BluetoothError as e:      
      errno = int(str(e).strip("(").split(",")[0])
      if errno!=77:
        wx.PostEvent(self._nw, ResultConnectionEvent(data=Comm.ERR_CONNECTION, errno=errno))
        wx.PostEvent(self._nw, ResultConnectionEvent(data=Comm.DISCONNECTED))
        return (time.time(), False)
 
    Comm.sock.settimeout(Comm.timeout)   
    wx.PostEvent(self._nw, ResultConnectionEvent(data=Comm.CONNECTED))
    return (time.time(), True)


  def _SendCmd(self,c):
    """ Send a command to the winsch and return the result (sample or parameter). """
    Comm.sock.send(chr(c))
    return self._Parse()

    
  def _ReadBytes(self,n):
    """ Waits for n number of bytes and return them. Or return None upon timeout. """  
    try:
      data = ""
      c = n
      while c>0:
        data += Comm.sock.recv(c)
        c -= len(data)
#        s = ""        
#        for i in data:
#          s += "%s," % i
#        print "data=%s type=%s len=%s" % (s, type(data), len(data))
#        print "type=%s len=%s" % (type(data), len(data))
      return data
    except bluetooth.BluetoothError:
      print "Timeout. Requested %d but got %d bytes." % (n, n-c)
      return None


  def _Parse(self):
    """ Reads bytes from the bluetooth buffer, parses and returns the result.
    None is returned if result cannot be properly parsed or there is a timeout. """
    o = None
    m = self._ReadBytes(1)  # First byte  represents the winsch mode
    #print "Mode is %d" % ord(m)
    if m==None or m=='':    # None returned upon timeout. Empty string when all data is read or connection closed. 
      return None
      #raise NameError('Timeout')

    m = struct.unpack('B', m[0])[0]
    if m==MODE.IDLE or m==MODE.TOWING:
      data = self._ReadBytes(10)
      if data==None:
        return None
      o = Sample()      
      o.Parse(m,data)
      return o
    
    if m==MODE.CONFIG_IS or m==MODE.CONFIG_OS:
      data = self._ReadBytes(34)
      if data==None:
        return None
      o = Parameter()    
      o.Parse(m,data)
      return o

    return None


  def Sync(self):
    """ Fetch parameters from winsch. """
    
    self.parameters = {}  
    o = self._SendCmd(CMD.SET)
    while (o.index not in self.parameters):
      self.parameters[o.index] = o
      o = self._SendCmd(CMD.SET)
      #    time.sleep(7)
      #    o = self._SendCmd(CMD.CONF)
      #    while (o.index not in self.parameters):
      #      self.parameters[o.index] = o
      #      o = self._SendCmd(CMD.SET)
      
    
    Sample.p_drum = self.parameters[0]    
    Sample.p_pump = self.parameters[1]    
    Sample.p_temp = self.parameters[2]    
    #Sample.p_pres = self.parameters[]    
      
    print "Done!"
      
#  def SETP(self):
#    self._SendCmd(CMD.SETP)


class SimulatedSocket():
  """ A dummy class to simulate a bluetooth connection """
  def __init__(self):
    self.cmd = CMD.NOCMD
    self.count = 0;
    self.paramno = -1; 
  
  def close(self, *args):
    pass

  def shutdown(self, *args):
    pass
  
  def connect(self, *args):
    time.sleep(1)

  def send(self, cmd):
    self.cmd = ord(cmd)
    #print "send(%d)" % self.cmd    
    time.sleep(0.18)
  
  def settimeout(self, *args):   
    pass

  def recv(self, c):
    #print "recv(%d)" % c
      
    if self.cmd==CMD.GET:
      if c==1:
        self.paramno = -1
        data = bytearray(1)
        data[0] = MODE.IDLE
        return str(data)
        
      self.count = self.count + 0.1
      data = bytearray(10)
      data[0:4] = [0,0,0,4]
      data[4] = int(127 + 128*np.sin(self.count))
      data[5] = int(127 + 128*np.sin(self.count+1))
      data[6:8] = [0,70]
      data[8:10] = [0,0]
      return str(data)
      
    if self.cmd==CMD.SET:
      if c==1:
        self.paramno += 1
        if self.paramno>5:  # Send max 5 parameters
          self.paramno = 0
        data = bytearray(1)
        data[0] = MODE.CONFIG_OS
        return str(data)
        
      self.count = self.count + 0.1
      data = bytearray(34)
      
      print "Sending param %d" % self.paramno
      data[0]     = self.paramno  # index
      data[1:3]   = [0,1]         # val
      data[3:5]   = [0,2]         # low
      data[5:7]   = [0,4]         # high
      data[7:9]   = [0,4]         # low_map
      data[9:11]  = [0,8]         # high_amp
      data[11:13] = [0,1]         # step
      data[13:34] = "abcabcabcabcabcabcabc"
      
      return str(data)
    
#    else:
#      return "."
